
//#include <msp430.h>
#include <msp430x552x.h>

#include <string.h>
#include "init.h"
#include "gyro_init.h"
#include "accel_init.h"
#include "compass_init.h"
#include "motor.h"


extern volatile unsigned int contatore;

/*
 * main.c
 */
void main(void) {
	volatile unsigned int i;
	volatile unsigned char valore;
	char conv[30], stato;
	gyro G;
	raw_data R;



	// Stop watchdog timer
    WDTCTL = WDTPW | WDTHOLD;

    /// set Internal DCO. This value is 2^15*3^2*5^2*3, so it is a mutiple of 2^9*3^2*5^2 = 115200
    setDCO(FDCO);

    /// initialize UART1
    initUART1(14400, FDCO);

    /// init port 1
    initPort1();
    P1REN = 2;			/// 0 0 0 0 0 0 1 0  pull up
    P1OUT = 2;

    P4DIR = 0x80;
    P4OUT = 0x80;

    initADC(10);
    initTIMER(FDCO);

	// Enable interrupt
	 __bis_SR_register(GIE);

	/// wait until P1.1 isn't pressed
	//while(P1IN & BIT1);
	caricaVettore();

	/*					INIT GYRO REGISTER			*/
	G.IsOn = OFF;
	initGyro(&G);
    while(1){

    	/// se sono passati 50 ms legge il giroscopio
    	if (contatore != 0){
    		readSensor(&G, &R);
    		contatore = 0;
    	}
    	/// letto il giroscopio provvede a leggere eventuali comandi da inviare
    	/// ai motori
    	receiveCommand();
    	for ( i = 0; i < 60000; i++);

    }
}






